using System;
using System.Collections.Generic;
using System.Text;
using System.Threading;
using OpenNETCF.IO.Serial;
using OpenNETCF.IO.Ports;
using System.Collections;

namespace edu.stanford.hci.ideas
{
    class SerialInterface
    {
        private const int COMMA = 0x2C;
        private const int ARRAY_SIZE = 1024;

        private static SerialPort serialPort;
        private Ranger ranger;
       
        public SerialInterface(Ranger r)
        {
            ranger = r;
        }

        public void OpenSerialPort()
        {
            String portName = "COM3";
            int baudRate = 115200;
            serialPort = new SerialPort(portName, baudRate);
            serialPort.Open();
            serialPort.DiscardNull = true;
            serialPort.ReceivedBytesThreshold = 2;
            //?
            //serialPort.ReadTimeout=50; not implemented
            //Assert.IsTrue( serialPort.IsOpen );
            serialPort.ReceivedEvent += new SerialReceivedEventHandler(serialPort_ReceivedEvent);
            serialPort.ErrorEvent += new SerialErrorEventHandler(serialPort_ErrorEvent);
        }

        private void serialPort_ErrorEvent(object sender, SerialErrorEventArgs e)
        {
            serialPort.Close();
            serialPort.Dispose();
            Thread.Sleep(40);
            OpenSerialPort();
        }

        /**
         * This function goes to the front of the values index by looking for the first null character, and then skipping to the next word-aligned index after that null character.
         */
        public static int skipToValues(byte[] b, int position)
        {
            while (b[position] != 0x00)
                position++;
            return (position + 3) & ~3;
        }

        public static int skipToAlign(int position)
        {
            return (position + 3) & ~3;
        }

        private void serialPort_ReceivedEvent(object sender, SerialReceivedEventArgs e)
        {
            byte[] bold = new byte[ARRAY_SIZE];

            int btr = serialPort.BytesToRead;
            serialPort.Read(bold, 0, btr);

            int position = 0;

            ArrayList args;
            int currentTypeIndex, currentValueIndex;
            byte type;
            //byte[] blob;

            // keep reading until start byte is reached
            while (bold[position++] != 0xBE) ;

            // next byte: length of message
            int length = (int)bold[position++];

            byte[] b = new byte[ARRAY_SIZE];
            Array.Copy(bold, position, b, 0, ARRAY_SIZE - position);

            position = 0;

            // next several bytes: address (null-terminated)
            String address = "";
            while (b[position] != 0x2C && b[position] != 0x00)
            {
                address += (char)b[position++];
            }
            int rangerIndex = Int32.Parse(address.Substring(4)) - 1;

            // keep reading until comma is reached
            while (b[position++] != COMMA) ;

            currentTypeIndex = position;
            position = skipToValues(b, position);
            currentValueIndex = position;
            args = new ArrayList(currentValueIndex - currentTypeIndex);

            bool loopDone = false;

            do
            {
                position = currentTypeIndex++;
                type = b[position];
                position = currentValueIndex;
                switch (type)
                {
                    case 0x69:			// 'i'  int
                        //args.add(new Integer(b.getInt()));
                        int value = b[position + 3] + (b[position + 2] << 8) + (b[position + 1] << 16) + (b[position] << 24);
                        ranger.setRangerData(rangerIndex, ((float)value)/255.0f);
                        position += 4;
                        currentValueIndex += 4;
                        break;
                    case 0x66:			// 'f'  float
                        //args.add(new Float(b.getFloat()));
                        currentValueIndex += 4;
                        break;
                    case 0x73:			// 's'  string
                    case 0x53:			// 'S'  symbol
                        //args.add(readString(b));
                        position = skipToAlign(position);
                        currentValueIndex = position;
                        break;
                    case 0x68:			// 'h'  long
                        //args.add(new Long(b.getLong()));
                        currentValueIndex += 8;
                        break;
                    case 0x64:			// 'd'  double
                        //args.add(new Double(b.getDouble()));
                        currentValueIndex += 8;
                        break;
                    case 0x62:			// 'b'  blob (raw data)
                        //blob = new byte[b.getInt()];
                        //b.get(blob);
                        position++; // get() increments position
                        //args.add(blob);
                        position = skipToAlign(position);
                        currentValueIndex = position;
                        break;
                    case 0x00:
                        loopDone = true;
                        break;
                    default:
                        Console.WriteLine("Unrecognized type!");
                        throw new Exception("Exception");
                }
            } while (!loopDone);
        }
        private void ListenToSerialPort(object sender, System.EventArgs e)
        {
            OpenSerialPort();
        }

    }
}
